using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using System.Threading;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Trees
{
    partial struct Tree
    {
        public class MultithreadedIntertreeTest<TOverlapHandler> where TOverlapHandler : struct, IOverlapHandler
        {
            struct Job
            {
                public int A;
                public int B;
            }

            public BufferPool Pool;

            int NextNodePair;
            int leafThreshold;
            private QuickList<Job> jobs;
            public int JobCount => jobs.Count;
            public Tree TreeA;
            public Tree TreeB;
            public TOverlapHandler[] OverlapHandlers;

            public MultithreadedIntertreeTest(BufferPool pool)
            {
                Pool = pool;
            }

            /// <summary>
            /// 准备与自检关联的作业。必须在通过PairTest进行调度之前调用。
            /// </summary>
            /// <param name="overlate Handler">用于处理自检检测到的单个重叠的回调。</param>
            /// <param name="threadcount">要准备作业的线程数。</param>
            public unsafe void PrepareJobs(ref Tree treeA, ref Tree treeB, TOverlapHandler[] overlapHandlers, int threadCount)
            {
                if (treeA.leafCount == 0 || treeB.leafCount == 0)
                {
                    // 如果任一棵树都没有树叶,则不需要树间测试。
                    // 由于此上下文具有用于计划目的的Count属性,该属性可读取作业列表,因此请清除该属性以确保不执行任何虚假作业。
                    jobs = new QuickList<Job>();
                    return;
                }
                Debug.Assert(overlapHandlers.Length >= threadCount);
                const float jobMultiplier = 1.5f;
                var targetJobCount = Math.Max(1, jobMultiplier * threadCount);
                // TODO：这个树间的树叶阈值没有投入太多的考虑。也许是更好的选择。
                leafThreshold = (int)((treeA.leafCount + treeB.leafCount) / targetJobCount);
                jobs = new QuickList<Job>((int)(targetJobCount * 2), Pool);
                NextNodePair = -1;
                this.OverlapHandlers = overlapHandlers;
                this.TreeA = treeA;
                this.TreeB = treeB;
                // 收集工作。
                if (treeA.leafCount >= 2 && treeB.leafCount >= 2)
                {
                    // 这两棵树都有完整的节点;我们可以使用一般情况。
                    GetJobsBetweenDifferentNodes(ref treeA.Nodes[0], ref treeB.Nodes[0], ref OverlapHandlers[0]);
                }
                else if (treeA.leafCount == 1 && treeB.leafCount >= 2)
                {
                    // 树A是退化的;需要特殊情况。
                    ref var a = ref treeA.Nodes[0];
                    ref var b = ref treeB.Nodes[0];
                    var aaIntersects = Intersects(a.A, b.A);
                    var abIntersects = Intersects(a.A, b.B);
                    if (aaIntersects)
                    {
                        DispatchTestForNodes(ref a.A, ref b.A, ref OverlapHandlers[0]);
                    }
                    if (abIntersects)
                    {
                        DispatchTestForNodes(ref a.A, ref b.B, ref OverlapHandlers[0]);
                    }
                }
                else if (treeA.leafCount >= 2 && treeB.leafCount == 1)
                {
                    // 树B是退化的;需要特殊情况。
                    ref var a = ref treeA.Nodes[0];
                    ref var b = ref treeB.Nodes[0];
                    var aaIntersects = Intersects(a.A, b.A);
                    var baIntersects = Intersects(a.B, b.A);
                    if (aaIntersects)
                    {
                        DispatchTestForNodes(ref a.A, ref b.A, ref OverlapHandlers[0]);
                    }
                    if (baIntersects)
                    {
                        DispatchTestForNodes(ref a.B, ref b.A, ref OverlapHandlers[0]);
                    }
                }
                else
                {
                    Debug.Assert(treeA.leafCount == 1 && treeB.leafCount == 1);
                    if (Intersects(treeA.Nodes[0].A, treeB.Nodes[0].A))
                    {
                        DispatchTestForNodes(ref treeA.Nodes[0].A, ref treeB.Nodes[0].A, ref OverlapHandlers[0]);
                    }
                }

            }

            /// <summary>
            /// 在多线程自检后清除。
            /// </summary>
            public void CompleteTest()
            {
                // 请注意,如果没有任何作业,我们不会分配作业列表。
                if (jobs.Span.Allocated)
                    jobs.Dispose(Pool);
            }

            public unsafe void ExecuteJob(int jobIndex, int workerIndex)
            {
                ref var overlap = ref jobs[jobIndex];
                if (overlap.A >= 0)
                {
                    if (overlap.B >= 0)
                    {
                        // 不同的内部节点。
                        TreeA.GetOverlapsBetweenDifferentNodes(ref TreeA.Nodes[overlap.A], ref TreeB.Nodes[overlap.B], ref TreeB, ref OverlapHandlers[workerIndex]);
                    }
                    else
                    {
                        // A是内部节点,B是叶。
                        var leafIndex = Encode(overlap.B);
                        ref var leaf = ref TreeB.Leaves[leafIndex];
                        ref var childOwningLeaf = ref Unsafe.Add(ref TreeB.Nodes[leaf.NodeIndex].A, leaf.ChildIndex);
                        TreeA.TestNodeAgainstLeaf(overlap.A, leafIndex, ref childOwningLeaf.Min, ref childOwningLeaf.Max, ref OverlapHandlers[workerIndex]);
                    }
                }
                else
                {
                    // A是一片叶子,B是内部的。
                    var leafIndex = Encode(overlap.A);
                    ref var leaf = ref TreeA.Leaves[leafIndex];
                    ref var childOwningLeaf = ref Unsafe.Add(ref TreeA.Nodes[leaf.NodeIndex].A, leaf.ChildIndex);
                    TreeA.TestLeafAgainstNode(leafIndex, ref childOwningLeaf.Min, ref childOwningLeaf.Max, overlap.B, ref TreeB, ref OverlapHandlers[workerIndex]);

                    // 请注意,我们不处理A和B都是叶子的情况。
                    // 收集例程应该会注意到这一点,因为它可以更方便地访问边界框,而且单个测试不值得原子增量。
                }
            }
            /// <summary>
            /// 执行多线程自检的单个辅助进程。
            /// </summary>
            /// <param name="workerIndex">执行这组测试的工作进程的索引。</param>
            public unsafe void PairTest(int workerIndex)
            {
                Debug.Assert(workerIndex >= 0 && workerIndex < OverlapHandlers.Length);
                int nextNodePairIndex;
                // 要最大限度地减少工作进程重叠列表的数量,请通过手动抓取下一个索引来执行直接负载平衡。
                while ((nextNodePairIndex = Interlocked.Increment(ref NextNodePair)) < jobs.Count)
                {
                    ExecuteJob(nextNodePairIndex, workerIndex);
                }
            }

            unsafe void DispatchTestForLeaf(ref Tree nodeOwner, int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, int nodeLeafCount, ref TOverlapHandler results)
            {
                if (nodeIndex < 0)
                {
                    // 维护树木秩序。树A的叶子应该始终是第一个参数。
                    if (Tree.Equals(nodeOwner, TreeA))
                        results.Handle(Encode(nodeIndex), leafIndex);
                    else
                        results.Handle(leafIndex, Encode(nodeIndex));
                }
                else
                {
                    if (nodeLeafCount <= leafThreshold)
                    {
                        // 维护树木秩序。树A的叶子应该始终是第一个参数。
                        if (Tree.Equals(nodeOwner, TreeA))
                            jobs.Add(new Job { B = Encode(leafIndex), A = nodeIndex }, Pool);
                        else
                            jobs.Add(new Job { A = Encode(leafIndex), B = nodeIndex }, Pool);
                    }
                    else
                        TestLeafAgainstNode(ref nodeOwner, leafIndex, ref leafMin, ref leafMax, nodeIndex, ref results);
                }
            }

            unsafe void TestLeafAgainstNode(ref Tree nodeOwner, int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results)
            {
                ref var node = ref nodeOwner.Nodes[nodeIndex];
                ref var a = ref node.A;
                ref var b = ref node.B;
                // 尽管进行了递归,Leaf Bound应该保持在L1中-它将从这里一直沿递归使用。
                // 然而,虽然我们可能在加载子A时加载子B,但不能保证它会一直存在。
                // 与将派生数据保留在堆栈上相比,在驱逐情况下重新加载它需要更多的工作。
                // TODO：这是一些非常有问题的微调。发现叶子之后的递归长度足以驱逐L1的情况并不常见。一定要测试一下。
                var bIndex = b.Index;
                var bLeafCount = b.LeafCount;
                var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max);
                var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max);
                if (aIntersects)
                {
                    DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafMin, ref leafMax, a.Index, a.LeafCount, ref results);
                }
                if (bIntersects)
                {
                    DispatchTestForLeaf(ref nodeOwner, leafIndex, ref leafMin, ref leafMax, bIndex, bLeafCount, ref results);
                }
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            unsafe void DispatchTestForNodes(ref NodeChild a, ref NodeChild b, ref TOverlapHandler results)
            {
                if (a.Index >= 0)
                {
                    if (b.Index >= 0)
                    {
                        if (a.LeafCount + b.LeafCount <= leafThreshold)
                            jobs.Add(new Job { A = a.Index, B = b.Index }, Pool);
                        else
                            GetJobsBetweenDifferentNodes(ref TreeA.Nodes[a.Index], ref TreeB.Nodes[b.Index], ref results);

                    }
                    else
                    {
                        // 叶B对节点A。
                        TestLeafAgainstNode(ref TreeA, Encode(b.Index), ref b.Min, ref b.Max, a.Index, ref results);
                    }
                }
                else if (b.Index >= 0)
                {
                    // 叶A对节点B。
                    TestLeafAgainstNode(ref TreeB, Encode(a.Index), ref a.Min, ref a.Max, b.Index, ref results);
                }
                else
                {
                    // 两片叶子。
                    results.Handle(Encode(a.Index), Encode(b.Index));
                }
            }

            unsafe void GetJobsBetweenDifferentNodes(ref Node a, ref Node b, ref TOverlapHandler results)
            {
                // 没有共享的子项,因此请将其全部测试。

                ref var aa = ref a.A;
                ref var ab = ref a.B;
                ref var ba = ref b.A;
                ref var bb = ref b.B;
                var aaIntersects = Intersects(aa, ba);
                var abIntersects = Intersects(aa, bb);
                var baIntersects = Intersects(ab, ba);
                var bbIntersects = Intersects(ab, bb);

                if (aaIntersects)
                {
                    DispatchTestForNodes(ref aa, ref ba, ref results);
                }
                if (abIntersects)
                {
                    DispatchTestForNodes(ref aa, ref bb, ref results);
                }
                if (baIntersects)
                {
                    DispatchTestForNodes(ref ab, ref ba, ref results);
                }
                if (bbIntersects)
                {
                    DispatchTestForNodes(ref ab, ref bb, ref results);
                }

            }
        }

    }
}
